
import time
from pymavlink import mavutil
from test.test import MAVLink

# 创建MAVLink连接

master = mavutil.mavlink_connection('/dev/tty.usbmodem21472242322', baud=115200)
file = 0
test = MAVLink(file)

def send_heartbeat():
    # mavlink官方电池pack
    # battery_status = master.mav.battery_status_encode(
    # id=0,  # 电池ID
    # # MAV_BATTERY_FUNCTION_UNKNOWN,MAV_BATTERY_FUNCTION_ALL
    # battery_function=mavutil.mavlink.MAV_BATTERY_FUNCTION_ALL,
    # type=mavutil.mavlink.MAV_BATTERY_TYPE_LIPO,
    # temperature=300,  # 温度（摄氏度）
    # voltages=[12000, 12000, 12000, 0, 0, 0, 0, 0, 0, 0],  # 电压（mV）
    # current_battery=5000,  # 电流（mA）
    # current_consumed=1000,  # 消耗的电流（mAh）
    # energy_consumed=500,  # 消耗的能量（mWh）
    # battery_remaining=80,  # 剩余电量（百分比）
    # )

    # 自定义fdb_info
    # fdb_info = test.pl_fdb_info_encode(0,0,0,33,0)
    # buf = fdb_info.pack(master.mav)
    # print(f"Encoded Buffer: {[hex(byte) for byte in buf]}")

    # 自定义host_info
    host_info = test.pl_host_info_encode(2,2,2,[0,0,20,0,0,20])
    # host_info = test.pl_host_info_encode(0,0,0,[0,0,20,0,0,20])

    # 发送消息
    master.mav.send(host_info)

# 接收并处理MAVLink消息
def receive_messages():
    msg = master.recv_match(blocking=True, timeout=1)
    if msg:
        # 打印解码后的消息内容
        decoded_msg = test.decode(msg.get_msgbuf())
        print("-----------------------------------")
        print(f"{msg._msgbuf[4]}")
        items = list(vars(decoded_msg).items())[11:]  # 忽略前11项系统属性
        for key, value in items:
            if key == "error_code":
                print(f"{key}: {hex(value)}")
            else:
                print(f"{key}: {value}")

# 主程序
if __name__ == "__main__":
    while True:
        send_heartbeat()
        receive_messages()
        
        time.sleep(0.01)
